/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 11:37:56
 * @LastEditor: junhuizhou
 * @LastEditTime: 2021-01-09 10:36:06
 * @Description: header
 * @FilePath: \fzhtq\src\caldrive.c
 */

#include <math.h>
#include "../include/caldrive.h"
#include "../include/constant.h"
#include "../include/calmatrix.h"

static int wc = 0;

void CalAttCtrlPS(double Mps[3], double thrusterAtt[3][10], double pstime[10])
{
    int i;
	double Fps[10];
	for(int i=0; i<10; i++)
	{
		if(pstime[i] > 3e-2)
		{
			Fps[i] = PS_F_Max;
		}
		else if(pstime[i] > 1e-2)
		{
			Fps[i] = 0.5*pstime[i]*1000 - 5;
		}
		else
		{
			Fps[i] = 0.0;
		}
	}
    MatMtp(Mps, (double*)thrusterAtt, 3, 10, Fps, 10, 1);
}

void CalWhl(double WhlSpdNow[4], double WhlSpdDsrd[4], double WhlSpdDsrdLst[4], double whlAxiAngMom[3], double WhlAxisTq[3])
{
    /*动量轮安装矩阵*/
	double whlDeployMat[3][4] =
		{
			{-1.0, 0.0, 0.0, 0.57735027},
			{0.0, -1.0, 0.0, 0.57735027},
			{0.0, 0.0, -1.0, 0.57735027}};
    double WhlAngMom[4]; /*动量轮角动量*/
    double WhlAccel[4];		 /*动量轮加速度*/
	double WhlTq[4];		 /*单个动量轮控制力矩*/
	double dSpd[4];			 /*动量轮转速增量（迭代周期）*/
	double deltaWhlSpd[4];   /*单个动量轮转速增量*/

    MatSub(deltaWhlSpd, WhlSpdDsrd, WhlSpdDsrdLst, 4, 1);
	double tmp = 1.0 / 2.0;
	/*动量轮角加速度*/
	MatMtp(WhlAccel, deltaWhlSpd, 4, 1, &tmp, 1, 1);
	/*根据不同控制模式选择不同的动量轮模型，计算动量轮力矩输出*/
	CalWhlTq(WhlTq, WhlAccel, MomWheel_Inertia);
	/*分20个周期，把动量轮角速度加到当前控制周期期望转速*/
	tmp = 1.0 / Ratio_Ctl2Sim;
	MatMtp(dSpd, deltaWhlSpd, 4, 1, &tmp, 1, 1);
	MatAdd(WhlSpdNow, WhlSpdNow, dSpd, 4, 1);
    /*如果当前仿真周期为控制周期中最后一个仿真周期，则强制赋值，使动量轮转速等于期望转速*/
	wc += 1;
	if (wc >= Ratio_Ctl2Sim)
	{
		for (int i = 0; i < 4; i++)
			{
				WhlSpdNow[i] = WhlSpdDsrd[i];
				WhlSpdDsrdLst[i] = WhlSpdDsrd[i];
			}
		wc = 0;
	}
	/*动量轮转速饱和*/
	for(int i=0; i<4; i++)
	{
		if(WhlSpdNow[i] > MomWheel_Spd_Max) WhlSpdNow[i] = MomWheel_Spd_Max;
		if(WhlSpdNow[i] < -MomWheel_Spd_Max) WhlSpdNow[i] = -MomWheel_Spd_Max;
		if(WhlSpdDsrdLst[i] > MomWheel_Spd_Max) WhlSpdDsrdLst[i] = MomWheel_Spd_Max;
		if(WhlSpdDsrdLst[i] < -MomWheel_Spd_Max) WhlSpdDsrdLst[i] = -MomWheel_Spd_Max;
	}
	/*由单个动量轮力矩计算三轴力矩*/
	MatMtp(WhlAxisTq, (double*)whlDeployMat, 3, 4, WhlTq, 4, 1);
	/*根据不同的控制模式，计算动量轮角动量*/
	CalWhlMom(WhlAngMom, WhlSpdNow, MomWheel_Inertia);
	/*由单个动量轮角动量计算三轴角动量*/
	MatMtp(whlAxiAngMom, (double*)whlDeployMat, 3, 4, WhlAngMom, 4, 1);
}

void CalWhlTq(double whlTq[4], double whlacc[4], double ratio)
{
    int i;
    for(i=0; i<4; i++)
    {
        whlTq[i] = -whlacc[i]*ratio;
    }
}

void CalWhlMom(double whlangmom[4], double whlspdnow[4], double ratio)
{
    int i;
    for(i=0; i<4; i++)
    {
        whlangmom[i] = whlspdnow[i]*ratio;
    }
}
